from machine import SPI, Pin
import utime
import st7789
import urandom
import math
from ulab import numpy as np


class Turtle:
    def __init__(self, display, x=120, y=120, backgroud_clr=st7789.color565(0, 0, 0)):
        self.backgroud_clr = backgroud_clr
        self.display = display
        self.zero_x = x
        self.zero_y = y
        self.last_x = x
        self.last_y = y
        self.now_x = x
        self.now_y = y
        self.pen = True
        self.turtle = False
        self.degree = 90
        self.size = 1
        self.pclr = st7789.color565(255, 255, 255)
        self.tclr = st7789.color565(0, 0, 0)
        self.display.init()
        self.display.fill(backgroud_clr)

        self.fill_clr = st7789.color565(255, 255, 255)
        self.fill_points = []
        self.beg_fill = False
        self.fill_circle = False

        self.m = "standard"

    def goto(self, x, y):
        if (self.turtle):
            self.drawturtle(self.degree, self.backgroud_clr)
        self.last_x = self.now_x
        self.last_y = self.now_y
        self.now_x = x + self.zero_x
        self.now_y = y + self.zero_y
        if (self.pen):
            self.drawline()
        if (self.turtle):
            self.drawturtle(self.degree, self.tclr)
        self.last_x = self.now_x
        self.last_y = self.now_y
        if (self.beg_fill):
            self.fill_points.append([self.now_x, self.now_y])
        return True

    def penup(self):
        self.pen = False
        return True

    def pendown(self):
        self.pen = True

    def left(self, degree):
        self.degree = (self.degree - (degree) % 360 + 360) % 360
        return True

    def right(self, degree):
        self.degree = (self.degree + (degree) % 360 + 360) % 360
        return True

    def drawline_direct_a(self, x1, y1, x2, y2, color, size):
        q = np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
        # print(q)
        if (q != 0):
            ux = (x2 - x1) / q
            uy = (y2 - y1) / q
            for l in np.arange(0, q, 0.5):
                px = x1 + l * ux
                py = y1 + l * uy
                self.display.fill_circle(int(px), int(py), size, color)
        return True

    def drawline_direct_b(self, x1, y1, x2, y2, color, size):
        self.display.fill_circle(x2, y2, int(size / 2), color)
        self.display.line(x1, y1, x2, y2, color)
        self.display.fill_circle(x1, y1, int(size / 2), color)
        lens2 = ((y2 - y1) ** 2) + ((x2 - x1) ** 2)
        lens = ((lens2) ** 0.5)
        r = size / 2
        if (lens != 0):
            qy = math.floor(r * (x2 - x1) / lens)
            qx = math.floor(r * (y2 - y1) / lens)

            self.display.line((x1 - qx), (y1 + qy), (x2 - qx), (y2 + qy), color)
            self.display.line((x1 + qx), (y1 - qy), (x2 + qx), (y2 - qy), color)

            nx1 = x2 - qx
            ny1 = y2 + qy
            nx2 = x2 + qx
            ny2 = y2 - qy

            lx1 = x1 - qx
            ly1 = y1 + qy
            lx2 = x1 + qx
            ly2 = y1 - qy

            l1 = []
            l2 = []
            q = np.sqrt((nx2 - nx1) ** 2 + (ny2 - ny1) ** 2)
            if (q != 0):
                ux = (nx2 - nx1) / q
                uy = (ny2 - ny1) / q
                for l in np.arange(0, q, 0.5):
                    px = nx1 + l * ux
                    py = ny1 + l * uy
                    l1.append([int(px), int(py)])
            if (q != 0):
                q = np.sqrt((lx2 - lx1) ** 2 + (ly2 - ly1) ** 2)
                ux = (lx2 - lx1) / q
                uy = (ly2 - ly1) / q
                for l in np.arange(0, q, 0.5):
                    px = lx1 + l * ux
                    py = ly1 + l * uy
                    l2.append([int(px), int(py)])
                for i in range(len(l1)):
                    self.display.line(l1[i][0], l1[i][1], l2[i][0], l2[i][1], color)
        return True

    def drawline(self):
        q = np.sqrt((self.now_x - self.last_x) ** 2 + (self.now_y - self.last_y) ** 2)
        if (q == 0):
            return
        ux = (self.now_x - self.last_x) / q
        uy = (self.now_y - self.last_y) / q
        for l in np.arange(0, q, 0.5):
            px = self.last_x + l * ux
            py = self.last_y + l * uy
            self.display.fill_circle(int(px), int(py), int(self.size / 2), self.pclr)
        return True

    def drawturtle(self, degree, color):
        if (self.turtle or color == self.backgroud_clr):
            tsize = 13
            temp_x = int(math.sin(degree * math.pi / 180) * tsize)
            temp_y = int(math.cos(degree * math.pi / 180) * tsize)
            print(degree)
            print((degree + 90) % 360)
            temp_x1 = int(math.sin((degree + 90) % 360 * math.pi / 180) * ((tsize / 2) - 2))
            temp_y1 = int(math.cos((degree + 90) % 360 * math.pi / 180) * ((tsize / 2) - 2))
            temp_x2 = int(math.sin((degree + 270) % 360 * math.pi / 180) * ((tsize / 2) - 2))
            temp_y2 = int(math.cos((degree + 270) % 360 * math.pi / 180) * ((tsize / 2) - 2))
            ax = temp_x + self.now_x
            ay = temp_y + self.now_y
            bx = temp_x1 + self.now_x
            by = temp_y1 + self.now_y
            cx = temp_x2 + self.now_x
            cy = temp_y2 + self.now_y
            self.display.line(bx, by, ax, ay, color)
            self.display.line(cx, cy, bx, by, color)
            self.display.line(ax, ay, cx, cy, color)

            q = np.sqrt((bx - cx) ** 2 + (by - cy) ** 2)
            ux = (bx - cx) / q
            uy = (by - cy) / q
            for l in np.arange(0, q, 0.5):
                px = cx + l * ux
                py = cy + l * uy
                self.display.line(int(px), int(py), int(ax), int(ay), color)
        return True

    def forward(self, distance):
        if (self.turtle):
            self.drawturtle(self.degree, self.backgroud_clr)
        temp_x = int(math.sin(self.degree * math.pi / 180) * distance)
        temp_y = int(math.cos(self.degree * math.pi / 180) * distance)

        self.now_x = temp_x + self.last_x
        self.now_y = temp_y + self.last_y

        if (self.pen):
            self.drawline()
        self.last_x = self.now_x
        self.last_y = self.now_y
        if (self.turtle):
            self.drawturtle(self.degree, self.tclr)
        if (self.beg_fill):
            self.fill_points.append([self.now_x, self.now_y])
        return True

    def pencolor(self, color):
        self.pclr = color
        return True

    def pensize(self, size):
        self.size = size
        return True

    def fillcolor(self, color):
        self.fill_clr = color
        return True

    def begin_fill(self):
        self.fill_points = [[self.now_x, self.now_y]]
        self.beg_fill = True
        return True

    def end_fill(self):
        if (self.fill_circle == False):

            Zx = 0
            Zy = 0
            for j in range(len(self.fill_points)):
                Zx = self.fill_points[j][0] + Zx
                Zy = self.fill_points[j][1] + Zy
            Zx = math.floor(Zx / len(self.fill_points))
            Zy = math.floor(Zy / len(self.fill_points))

            for i in range(len(self.fill_points)):
                if (i > 0):

                    q = np.sqrt((self.fill_points[i][0] - self.fill_points[i - 1][0]) ** 2 + (
                                self.fill_points[i][1] - self.fill_points[i - 1][1]) ** 2)
                    ux = (self.fill_points[i][0] - self.fill_points[i - 1][0]) / q
                    uy = (self.fill_points[i][1] - self.fill_points[i - 1][1]) / q
                    for l in np.arange(0, q, 0.5):
                        px = self.fill_points[i - 1][0] + l * ux
                        py = self.fill_points[i - 1][1] + l * uy
                        self.drawline_direct_b(Zx, Zy, int(px), int(py), self.fill_clr, 3)

            self.fill_points = []
            self.beg_fill = False
        else:
            if (self.fill_points[0][3] >= 360):
                self.display.fill_circle(self.fill_points[0][0], self.fill_points[0][1], self.fill_points[0][2],
                                         self.pclr)
            else:
                Cx = self.fill_points[0][0]
                Cy = self.fill_points[0][1]
                R = self.fill_points[0][2]
                rad = self.fill_points[0][3]
                p1 = 0 * math.pi / 180
                p2 = rad * math.pi / 180
                dp = (p2 - p1) / (8 * R)

                for p in np.arange(p1, p2, dp):
                    px = Cx + R * math.sin(p)
                    py = Cy + R * math.cos(p)
                    self.drawline_direct_b(Cx, Cy, int(px), int(py), self.pclr, 1)
            self.fill_points = []
            self.beg_fill = False
        return True

    def setheading(self, angle):
        """立即把海龟朝向设为 angle 度，0/360=右，90=上，180=左，270=下"""
        self.degree = angle % 360
        return True